﻿using System;
using System.Collections.Generic;
using System.Text;
using FIRADriverLibrary;
using AGHFiraPlayer;
using GeometryCSharp;

namespace AGHFiraPlayer.PreviousDriver
{
    public abstract class SimpleMovingFunctions
    {
        public static void intersectBallTrajectory(RobotController robotController, FIRADriver fira)
        {
            double positionError = AGHDriver.ROBOT_DIAMETER;
            double maximumDistanceToKick = AGHDriver.ROBOT_DIAMETER;

            Point2D robotPosition = new Point2D(robotController.robotState.x,robotController.robotState.y);
            Point2D intersectionPoint = Predicator.getIntersectionPoint(robotPosition.x, robotPosition.y);

            Point2D  myPoint = new Point2D(intersectionPoint.x, intersectionPoint.y);
            Point2D ballPos = new Point2D(fira.ballX,fira.ballY);


            if (robotPosition.GetDistance(myPoint) > positionError){
                 rotateToBall(robotController, fira);
                 PotencialDriver.DriveVelocity(robotController.robotState, intersectionPoint.x, intersectionPoint.y, PotencialDriver.maxVelocity);
            }
            else
            {
                //robotController.Stop();
                rotateToBall(robotController, fira);
                if (robotPosition.GetDistance(ballPos)<maximumDistanceToKick)
                  rotateKick(robotController,true);
           };
            /*
            Vector2D robotBallVector = new Vector2D(fira.ballX - robotController.robotState.x, fira.ballY - robotController.robotState.y);
            double angleBallRobot = Vector2D.AngleBetween(Predicator.V, robotBallVector);
            robotBallVector.Rotate(-(Math.PI / 2 - Math.Abs(angleBallRobot)));
            Console.WriteLine(robotBallVector.Angle.ToString() + "   " + (Math.PI / 2 - Math.Abs(angleBallRobot)).ToString());

            double targetAngle = robotBallVector.Angle;
            rotate(robotController, targetAngle);
             */ 
        }


        public static void rotateToBall(RobotController robotController, FIRADriver fira)
        {
            Vector2D robotBallVector = new Vector2D(fira.ballX - robotController.robotState.x, fira.ballY - robotController.robotState.y);
            double angle = robotBallVector.Angle;
            rotate(robotController, angle);
        }


        //uzaleznic multiplicator w zaleznosci od kata
        public static void rotate(RobotController robotController, double robotBallAngle)
        {
            double diff = new Vector2D(robotController.robotState.angle).AngleBetween(new Vector2D(robotBallAngle));
            robotController.robotState.rightSpeed = 0;
            robotController.robotState.leftSpeed = 0;
            if (Math.Abs(diff) < 0.1)
                return;
            else
            {
                double multiplicator = (Math.Abs(diff) / Math.PI);
                robotController.robotState.leftSpeed = PotencialDriver.maxVelocity * multiplicator;
                robotController.robotState.rightSpeed = -PotencialDriver.maxVelocity * multiplicator;
            }
        }

        public static void intersectBallTrajectoryWithEndVector(RobotController robotController, FIRADriver fira)
        {
            //TODO
        }

        public static void rotateKick(RobotController robotController, bool right)
        {
                if (right)
                {
                    robotController.robotState.leftSpeed = PotencialDriver.maxVelocity;
                    robotController.robotState.rightSpeed = -PotencialDriver.maxVelocity;
                }
                else
                {
                    robotController.robotState.rightSpeed = PotencialDriver.maxVelocity ;
                    robotController.robotState.leftSpeed = -PotencialDriver.maxVelocity;
                }
 
            
        }
    }
}
